//
// Created by Hoyin on 2023/12/25.
//

#include "../include/RelativeOrientation.h"
using namespace Eigen;
using namespace std;


void ContinuousOrient::setParameters(double phi, double omega, double kappa, double mu, double nu){
	this->Phi = phi;
	this->Omega = omega;
	this->Kappa = kappa;
	this->Mu = mu;
	this->Nu = nu;
}


void ContinuousOrient::setPointPairs(const std::vector<cv::KeyPoint> &lKeyPoints,
                                     const std::vector<cv::KeyPoint> &rKeyPoints,
                                     const std::vector<cv::DMatch> &matches) {
	this->vPointPairs.clear();
	for (auto &match : matches) {
		Vector2d lKeyPoint(lKeyPoints[match.queryIdx].pt.x / _cam0_ppm_x, lKeyPoints[match.queryIdx].pt.y / _cam0_ppm_y);
		Vector2d rKeyPoint(rKeyPoints[match.trainIdx].pt.x / _cam1_ppm_x, rKeyPoints[match.trainIdx].pt.y / _cam1_ppm_y);
		ImageCoord leftIC(lKeyPoint, cam0_intrinsic);
		ImageCoord rightIC(rKeyPoint, cam1_intrinsic);
		PointPair<ImageCoord> pointPair(leftIC, rightIC);
		this->vPointPairs.emplace_back(pointPair.copy());
	}
}


void ContinuousOrient::solve() {
	uint32_t pairCount = this->vPointPairs.size();
//	改正数向量
	Vector<double, 5> X = Vector<double, 5>::Ones()*100;

	bool convergent = isConvergent(X, 0.3e-4);
	size_t iter = 0;
	while (!convergent) {
//		误差方程系数阵，自由项
		MatrixXd A = MatrixXd::Zero(pairCount, 5);
		VectorXd L = VectorXd::Zero(pairCount);

		Matrix3d E = Matrix3d::Identity();
//      右片像相对左片的旋转矩阵，包含9个方向余弦
		Matrix3d R = Rotation(Phi, Omega, Kappa).R;


//	    针对每一对同名像点，都可以列立一条共面条件方程
		for (int i = 0; i < pairCount; i++) {
//		    应用于当前方程的点对
			PointPair pp = this->vPointPairs.at(i);
//          点对的像空辅坐标
			ImageAssistCoord iacL(pp.LPoint, E);
			ImageAssistCoord iacR(pp.RPoint, R);
//		    By, Bz, N1, N2
			double By = Mu*Bx;
			double Bz = Nu*Bx;
			double X1 = iacL.X();
			double X2 = iacR.X();
			double Y1 = iacL.Y();
			double Y2 = iacR.Y();
			double Z1 = iacL.Z();
			double Z2 = iacR.Z();
			double N1 = (Bx * Z2 - Bz * X2) / (X1 * Z2 - Z1 * X2);
			double N2 = (Bx * Z1 - Bz * X1) / (X1 * Z2 - Z1 * X2);
//		    建立误差方程
			A(i, 0) = -(X2 * Y2 * N2) / Z2;
			A(i, 1) = -(Z2 + Y2 * Y2 / Z2) * N2;
			A(i, 2) = X2 * N2;
			A(i, 3) = Bx;
			A(i, 4) = -Y2 * Bx / Z2;
			L(i) = N1 * Y1 - N2 * Y2 - By;
		}
//	以上循环结束，所有误差方程系数及常数均算出，得到A, L

//      解改正数
		X = (A.transpose()*A).inverse()*A.transpose()*L;
//		学习率
		iter++;
		double a = 1.0 / double(iter) * 50;
		a = a > 1 ? 1 : a;
		Phi += X(0) * a;
		Omega += X(1) * a;
		Kappa += X(2) * a;
		Mu += X(3) * a;
		Nu += X(4) * a;
//		cout << "\r in iter " << iter << " ; learning rate: " << a << "    ";
		convergent = isConvergent(X, 0.3e-3);
		if (iter > 1000) return;
		double By = Mu*Bx;
		double Bz = Nu*Bx;
		Vector3d baseline(Bx, By, Bz);
		sta1_position = sta0_position + baseline;
	}
}

void ContinuousOrient::output() const {
	cout << endl;
	cout << "Phi: " << Phi << endl;
	cout << "Omega: " << Omega << endl;
	cout << "Kappa: " << Kappa << endl;
	cout << "Mu: " << Mu << endl;
	cout << "Nu: " << Nu << endl;
	Eigen::IOFormat iof(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[ ", " ]");
	cout << "RightStationPosition: " << endl << sta1_position.format(iof) << endl;
	cout << "Baseline Length: " << sta1_position.norm() << endl;
}

ContinuousOrient::Result ContinuousOrient::getResult() {
	ContinuousOrient::Result result;
	result.RotateMatrix = Rotation(Phi, Omega, Kappa).R;
	result.RStation = sta1_position;
	return result;
}
